#include "abs_encoder.h"

/* 
160cm 对应茶机高度中间位置 对应绝对编码器输出160 * 409.6  = 65536
电机25圈 对应编码器60圈
电机1圈 对应编码器2.4圈
电机转一圈 高度变化量对应5.416cm
推算出编码器转一圈 高度变化量2.2566666666666666666666666666667 约等于2.2567cm

编码器分辨率12位 4096变化量
即一圈对应4096 高度变化量2.2567cm对应4096 即2.2566666666666666666666666666667 对应4096
1cm高度变化量对应编码器变化量1815.0664697193500738552437223043 约等于1815

编码器32圈最大量程 对应最大测试距离72.2144cm 
茶陇高度活动范围77cm - 99cm 满足变化量22cm以上即可

//**************************************************************以上为按公式换算高度 实际测试误差太大*****************************************8**************

考虑采茶机对精度要求不高 现直接用绝对编码器控制量和既定高度建立对应关系
14.7cm对应编码器变化量4592
高：64571
低：59979

即1cm对应编码器变化量312.38095238095238095238095238095 约等于312
编码器转一圈4096 即1cm高度上下对应编码器0.07626488095238095238095238095238圈 约等于0.076圈（编码器转一圈，对应高度上下13.128205128205128205128205128205cm，约等于13cm）
电机1圈 对应编码器2.4圈 ---> 编码器转1圈 对应电机转0.41666666666666666666666666666667圈 约等于0.417圈
-->即高度上下1cm对应电机转0.03177703373015873015873015873016圈 约等于0.032圈
理论测试：假设转1cm 对应脉冲数：1600 * 0.031777 = 50.843253968253968253968253968256 约等于51个脉冲 51 * 0.225° = 11.475°
最小单位 1个脉冲 0.225° 对应电机0.003125圈 编码器0.0075圈 对应高度上下0.0975cm -->定为小于0.5cm则不动

实际测试（14.7cm）
192 382
207 397    
15 15cm ---基本一致 公式换算解决

总量程4096 * 32  = 131072
最大测试高度：131072 / 312 = 420.10256410256410256410256410256cm 即4米2
茶机总变化量22cm ---> 对应编码器变化量 6864
设置初始中位值10000 ----》即大约 13432  --- 6568
10000对应16进制2710

5cm往复测试
5cm对应编码器变化量1,561.9047619047619047619047619048 约等于1561
对应电机圈数0.1588851686507936507936507936508 约等于0.16
电机一圈对应1600个脉冲 即5cm对应输入脉冲数254.21626984126984126984126984128 约等于254  1cm 50左右
3cm 对应


最低点对应编码器实际位置 32cm 实际量出来57cm? 那偏差25cm
编码器相对折算cm是正确的
*/
uint8_t read_L_abs_encoder_cmd[6] = {0x01, 0x03, 0x00, 0x00, 0x00, 0x02};
uint8_t read_L_abs_encoder_cmd_crc[8] = {0x01, 0x03, 0x00, 0x00, 0x00, 0x02, 0xC4, 0x0B};
// uint8_t set_L_encoder_zero_cmd_crc[] = {0x01, 0x10, 0x00, 0x4A, 0x00, 0x02, 0x04, 0x00, 0x00, 0x00, 0x00, 0x77, 0xE0};
uint8_t read_R_abs_encoder_cmd_crc[8] = {0x02, 0x03, 0x00, 0x00, 0x00, 0x02, 0xC4, 0x38};
// uint8_t set_R_encoder_zero_cmd_crc[] = {0x02, 0x10, 0x00, 0x4A, 0x00, 0x02, 0x04, 0x00, 0x00, 0x00, 0x00, 0x78, 0xA4};
uint8_t set_R_encoder_addr_cmd_crc[] = {0x01, 0x06, 0x00, 0x44, 0x00, 0x02, 0x48, 0x1E};
uint8_t set_L_encoder_value_cmd_crc[] = {0x01, 0x10, 0x00, 0x4A, 0x00, 0x02, 0x04, 0x00, 0x00, 0x27, 0x10, 0x6D, 0xDC};
uint8_t set_R_encoder_value_cmd_crc[] = {0x02, 0x10, 0x00, 0x4A, 0x00, 0x02, 0x04, 0x00, 0x00, 0x27, 0x10, 0x62, 0x98};
uint16_t CRC_value_test = 0;
extern UART_HandleTypeDef huart3;
extern DMA_HandleTypeDef hdma_usart3_rx;
uint8_t huart3_rev_data[100] = {0};
int32_t abs_encoder_pos_L = 0; 
float real_pos_L = 0;
int32_t abs_encoder_pos_R = 0; 
float real_pos_R = 0;
uint16_t CRC_16_cal = 0;
uint64_t encoder_iqr_count_test = 0;
uint64_t encoder_test2 = 0;
void read_encoder_pos(void);
void set_R_encoder_addr(void);
void set_encoder_pos(void);

void abs_encoder_init(void)
{
    __HAL_UART_ENABLE_IT(&huart3, UART_IT_TC);
    // set_R_encoder_addr();
    read_encoder_pos();
    // for(uint8_t i = 0; i < 2; i ++)
    // {
    //   set_encoder_pos();
    //   HAL_Delay(100);
    // }
    __HAL_UART_ENABLE_IT(&huart3, UART_FLAG_IDLE);
    HAL_UART_Receive_DMA(&huart3, huart3_rev_data, sizeof(huart3_rev_data));

}

/*CRC16_MODBUS*/
void InvertUint16(unsigned short *dBuf,unsigned short *srcBuf)
{
	int i;
	unsigned short tmp[4]={0};
 
	for(i=0;i< 16;i++)
	{
		if(srcBuf[0]& (1 << i))
		tmp[0]|=1<<(15 - i);
	}
	dBuf[0] = tmp[0];
}


unsigned short CRC16_MODBUS(unsigned char *data, unsigned int datalen)
{
	unsigned short wCRCin = 0xFFFF;
	unsigned short wCPoly = 0x8005;
	
	InvertUint16(&wCPoly,&wCPoly);
	while (datalen--) 	
	{
		wCRCin ^= *(data++);
		for(int i = 0;i < 8;i++)
		{
			if(wCRCin & 0x01)
				wCRCin = (wCRCin >> 1) ^ wCPoly;
			else
				wCRCin = wCRCin >> 1;
		}
	}
	return (wCRCin);
}

//得出0x0BC4  加入应以C4 0B的方式加入
void CRC_cal_test(void)
{
    CRC_value_test = CRC16_MODBUS(read_L_abs_encoder_cmd, sizeof(read_L_abs_encoder_cmd));
}

void read_encoder_pos(void)
{
    static uint8_t i = 0;
    i ++;
    if(i == 1)
    {
      HAL_UART_Transmit_DMA(&huart3, read_L_abs_encoder_cmd_crc, sizeof(read_L_abs_encoder_cmd_crc));
    }
    else if(i == 2)
    {
      HAL_UART_Transmit_DMA(&huart3, read_R_abs_encoder_cmd_crc, sizeof(read_R_abs_encoder_cmd_crc));
      i = 0;
    }
    
}

void set_R_encoder_addr(void)
{
    HAL_UART_Transmit_DMA(&huart3, set_R_encoder_addr_cmd_crc, sizeof(set_R_encoder_addr_cmd_crc));
}

// void set_encoder_zero(void)
// {
//     HAL_UART_Transmit_DMA(&huart3, set_L_encoder_zero_cmd_crc, sizeof(set_L_encoder_zero_cmd_crc));
//     // HAL_UART_Transmit_DMA(&huart3, set_R_encoder_zero_cmd_crc, sizeof(set_R_encoder_zero_cmd_crc));
// }

void set_encoder_pos(void)
{
    // static uint8_t i = 0;
    // i ++;
    // if(i == 1)
    // {
    //   HAL_UART_Transmit_DMA(&huart3, set_L_encoder_value_cmd_crc, sizeof(set_L_encoder_value_cmd_crc));
    // }
    // else if(i == 2)
    // {
      HAL_UART_Transmit_DMA(&huart3, set_R_encoder_value_cmd_crc, sizeof(set_R_encoder_value_cmd_crc));
      // i = 0;
    // }
}


void encoder_data_deal(uint8_t rev_len)
{
    
    if(huart3_rev_data[0] == 0x01 && huart3_rev_data[1] == 0x03 && huart3_rev_data[2] == 0x04)
    {
      //CRC校验
      encoder_test2 ++;
      // CRC_16_cal = CRC16_MODBUS(huart3_rev_data, sizeof(huart3_rev_data) - 2);
      // if((CRC_16_cal >> 8 & 0xff) == huart3_rev_data[rev_len - 1] && (CRC_16_cal & 0xff) == huart3_rev_data[rev_len - 2])
      // {
        //校验通过后数据处理(计算需要时间 难处理过来 不校验)
        abs_encoder_pos_L = huart3_rev_data[3] << 24 | huart3_rev_data[4] << 16 | huart3_rev_data[5] << 8 | huart3_rev_data[6];
        real_pos_L = (float)abs_encoder_pos_L / 312; //对应形成距离 单位cm 分辨率4096
      // }
      
    } 

    else if(huart3_rev_data[0] == 0x02 && huart3_rev_data[1] == 0x03 && huart3_rev_data[2] == 0x04)
    {
      //CRC校验
      // CRC_16_cal = CRC16_MODBUS(huart3_rev_data, sizeof(huart3_rev_data) - 2);
      // if((CRC_16_cal >> 8 & 0xff) == huart3_rev_data[rev_len - 1] && (CRC_16_cal & 0xff) == huart3_rev_data[rev_len - 2])
      // {
        //校验通过后数据处理
        abs_encoder_pos_R = huart3_rev_data[3] << 24 | huart3_rev_data[4] << 16 | huart3_rev_data[5] << 8 | huart3_rev_data[6];
        real_pos_R = (float)abs_encoder_pos_R / 312; //对应形成距离 单位cm
      // }
      
    } 
}

void rev_encoder_data_analysis(void)
{
    int32_t read_temp = 0;
    int16_t rev_data_length_temp = 0;
    if(__HAL_UART_GET_FLAG(&huart3, UART_FLAG_IDLE) != RESET)
    {
        encoder_iqr_count_test++;
        __HAL_UART_CLEAR_FLAG(&huart3, UART_FLAG_IDLE);
        HAL_UART_DMAStop(&huart3); 
        read_temp = (&huart3)->Instance->SR;
        read_temp = (&huart3)->Instance->DR;
        rev_data_length_temp = sizeof(huart3_rev_data) - (&hdma_usart3_rx)->Instance->NDTR;
        encoder_data_deal(rev_data_length_temp);
        memset(huart3_rev_data, sizeof(huart3_rev_data), 0);
        HAL_UART_Receive_DMA(&huart3, huart3_rev_data, sizeof(huart3_rev_data));
    }
}

